//
// Created by ysbf on 8/21/20.
//

#ifndef BNO055DRIVER_SRC_MPU9250_MPU9250_H
#define BNO055DRIVER_SRC_MPU9250_MPU9250_H


#include "mpu9250_reg.h"

class MPU9250 {
public:

public:

#define DEGTORAD(x) ((x) * 0.01745329251994329576923690768489f)
  typedef void (*RWRegCallback)(uint16_t);
  typedef void(*ReadCallback)();
  typedef void (*spi_read_type)(const uint8_t *buffer, uint16_t len);
  typedef void (*spi_write_type)(const uint8_t *buffer, uint16_t len);
  typedef void (*spi_async_read_type)(const uint8_t *buffer, uint16_t len, void callback_fun(uint16_t));
  typedef void (*spi_async_write_type)(const uint8_t *buffer, uint16_t len, void callback_fun(uint16_t));
  typedef void (*delay_type)(uint16_t);

  MPU9250(spi_read_type spi_read, spi_write_type spi_write, spi_async_read_type spi_async_read,
          spi_async_write_type spi_async_write, delay_type delay);;

  //  template<typename Read_Handler>

  void async_read_reg(uint8_t reg, uint8_t &value, const RWRegCallback callback);;

  void async_read_regs(uint8_t reg, uint8_t *buffer, uint8_t len, const RWRegCallback callback);;

  void async_write_reg(uint8_t reg, uint8_t value, const RWRegCallback callback);;

  void async_write_regs(uint8_t reg, uint8_t *buffer, uint8_t len, const RWRegCallback callback);;

  uint8_t read_reg(uint8_t reg);

  void write_reg(uint8_t reg, uint8_t value);

  void read_regs(uint8_t reg, const uint8_t *buffer, uint8_t len);;

  void get_raw_data(int16_t *accel, int16_t *gyro, int16_t *mag);

  void get_9axis_data(float *accel, float *gyro, float *mag);

  //  template<typename Callback>
  void async_get_9axis_data(float *accel, float *gyro, float *mag, const ReadCallback callback);

  void async_get_raw_data(int16_t *accel, int16_t *gyro, int16_t *mag, const ReadCallback callback);;

  //void MPU9250_DeviceInit_2(uint8_t Ascale, uint8_t Gscale, uint8_t sampleRate)
  void init();

private:
  spi_read_type m_spi_read;
  spi_write_type m_spi_write;

  spi_async_read_type m_spi_async_read;
  spi_async_write_type m_spi_async_write;

  delay_type m_delay;
  uint8_t spi_buffer[33]{0};
};

#endif //BNO055DRIVER_SRC_MPU9250_MPU9250_H
